/*
 * describe: Attitude calculate
 * 姿态解算部分
 * author:项彦凯 yankaixiang
 * date: 2022-12-25
 * date: 2024-2-3 修正
 * date: 2024-2-12
 * @description:都什么年代了还在用祖传BMX055，换个新的吧！！！
 * @description: 只需要把陀螺仪的数据传入即可，特别注意角速度 加速度单位要换算！！
 * 单位是：加速度：arbitrary units g 角速度：  单位 degrees/s
 * @description: 理论上所有陀螺仪数据一股脑塞进去即可，参数不需要调，也支持磁力计。以后姿态解算都用这个吧。飞控上扒下来的，px4都在用，大疆军工方案也是这个
 * Attitude_Calculate是姿态解算函数 可以赛循环也可以塞中断 ，Attitude_Init陀螺仪飘初始化
 * 这个代码是给大家的姿态解算的DEMO 基于icm20602
 */
#include "Attitude.h"
#include "HeadFile.h"

//==================================================================================================
#define gyroscope_threshold 5
//--------------------------------------------------------------------------------------
    static float gyroscopeOffset[3] = {0.0f, 0.0f, 0.0f};    //gyro
    float gyroscope[3] ={0.0f,0.0f,0.0f};                   //gyro
    float accelerometer[3] = {0.0f, 0.0f, 0.0f};    //acc
    float magnetometer[3] = {0.0f, 0.0f, 0.0f};     //mag
    float eulerAngle[3] = {0.0f, 0.0f, 0.0f};       //euler angle
//--------------------------------------------------------------------------------------


//===================================================================================================

bool Attitude_Init()
{
    //===========================================================================================
    for (int i = 0; i < 1000; ++i) { //0漂初始化
        icm20602_get_acc();
        icm20602_get_gyro();

        //0漂初始化阈值判断
        if(fabsf(gyroscope[0])+ fabsf(gyroscope[1])+ fabsf(gyroscope[2])<gyroscope_threshold) {
            gyroscopeOffset[0] += icm20602_gyro_transition(icm20602_gyro_x);
            gyroscopeOffset[1] += icm20602_gyro_transition(icm20602_gyro_y);
            gyroscopeOffset[2] += icm20602_gyro_transition(icm20602_gyro_z);
        }
        else
        {
            i--;   //超过阈值了再取一次
        }
        system_delay_ms(2);  //按照自己的采样率来取值 demo是500Hz即2ms中断
    }
    gyroscopeOffset[0]/=1000;
    gyroscopeOffset[1]/=1000;
    gyroscopeOffset[2]/=1000;

    //=============================================================================================

    Mahony_Init(500);  //设置采样率  这个是2ms解算一次 采样率为500Hz
    icm20602_get_acc();  //利用加速度计 进行一个初始的大致位置计算。去掉这句也可以进行姿态解算但是位置收敛速度会很慢
    //默认也没加磁力计 如果有需要可以加一下，磁力计比较麻烦要进行校准
    MahonyAHRSinit(icm20602_acc_transition(icm20602_acc_x),icm20602_acc_transition(icm20602_acc_y),icm20602_acc_transition(icm20602_acc_z),0,0,0);  //上电快速开始初始化
    return true;
}

void Attitude_Calculate()
{
    // replace this with actual gyroscope data in degrees/s
    icm20602_get_acc();
    icm20602_get_gyro();
    //icm20602_get_mag();
    //要进行角度切换 具体角度单位请看下面的注释
    gyroscope[0]= icm20602_gyro_transition(icm20602_gyro_x); //replace this with actual accelerometer data in degrees/s
    gyroscope[1]= icm20602_gyro_transition(icm20602_gyro_y);
    gyroscope[2]= icm20602_gyro_transition(icm20602_gyro_z);
    accelerometer[0]=icm20602_acc_transition(icm20602_acc_x);
    accelerometer[1]=icm20602_acc_transition(icm20602_acc_y);
    accelerometer[2]=icm20602_acc_transition(icm20602_acc_z); // replace this with actual accelerometer data in g
    magnetometer[0]=icm20602_mag_transition(icm20602_mag_x);
    magnetometer[1]=icm20602_mag_transition(icm20602_mag_y);
    magnetometer[2]=icm20602_mag_transition(icm20602_mag_z);// replace this with actual magnetometer data in arbitrary units;
    //=================================================================
    //减去零飘 如果前面零漂去了 这里记得注释
    gyroscope[0]-= gyroscopeOffset[0];
    gyroscope[1]-= gyroscopeOffset[1];
    gyroscope[2]-= gyroscopeOffset[2];
//===================================================================================================
    //姿态解算 后三位为磁力计，一般不用就行 填0
    Mahony_update(gyroscope[0],gyroscope[1],gyroscope[2],accelerometer[0],accelerometer[1],accelerometer[2],0,0,0);
    Mahony_computeAngles(); //角度计算
    eulerAngle[0]=getPitch(); //获取Pitch
    eulerAngle[1]=getRoll(); //获取Roll
    eulerAngle[2]=getYaw();  //获取Yaw
}
